/*
 * main.cpp
 *
 *  Created on: Oct 27, 2012
 *      Author: bouchier
 */
/*! \file jRoverTest/main.cpp
 * \brief Header file for jRoverTest - the file which does whole-system exercising of jRover
 *
 * <H1>
 * Build Configuration
 * </H1>
 *
 * This project depends on the following peer projects being at the same directory level:
 * - CQEI2C
 * - Metro
 * - CQEIMEncoder
 * - qetime
 * - PID
 * - RCTest
 * - ControlledMotor
 * - JRoverMotors
 * - JRoverSensors
 * - ROSIF
 * - Runtimer
 *
 * Edit the project properties as follows to reference them as includes, link objects, and referenced projects.
 *
 * Under C/C++ Build -> Settings, Tool Settings tab, TerkOS C++ Compiler group, Directories settings
 * add the following paths
 * to the terkos paths that are already there. This adds them to the include path for compilation
 * - ../../CQEI2C
 * - ../../Metro
 * - ../../CQEIMEncoder
 * - ../../qetime
 * - ../../PID
 * - ../../RCTest
 * - ../../ControlledMotor
 * - ../../jrover_msgs/ros_lib/jrover
 * - ../../JRoverMotors
 * - ../../ros_lib
 * - ../../ROSIF
 * - ../../Runtimer
 *
 * Under C/C++ Build -> Settings, Tool Settings tab, TerkOS C++ Linker group, Miscellaneous settings, add
 * the following "other objects". This tells the linker to link to qetime.o & CQEI2C.o.
 * - ../../CQEI2C/Debug/CQEI2C.o
 * - ../../Metro/Debug/Metro.o
 * - ../../CQEIMEncoder/Debug/CQEIMEncoder.o
 * - ../../qetime/Debug/qetime.o
 * - ../../PID/Debug/pid.o
 * - ../../RCTest/Debug/RCTest.o
 * - ../../ControlledMotor/Debug/ControlledMotor.o
 * - ../../JRoverMotors/Debug/JRoverMotors.o
 * - ../../ROSIF/Debug/ROSIF.o
 * - ../../usonicRanger/Debug/usonicRanger.o
 * - ../../Runtimer/Debug/Runtimer.o
 *
 * In the Project References group, check the following projects. This builds them before the current project.
 * CQEI2C
 * Metro
 * CQEIMEncoder
 * qetime
 * PID
 * ControlledMotor
 * RCTest
 * JRoverMotors
 * ROSIF
 * usonicRanger
 *
 */

#define DEFAULT_TEST 3

// default command lines to run. Add new command lines here & comment out all but one
//char *default_argv[] = {(char*)"jRoverTest", (char*)"-r"};
//int default_argc = 2;
char *default_argv[] = {(char*)"jRoverTest"};
int default_argc = 1;

#include <stdio.h>
#include <getopt.h>
#include <keypad.h>
#include <textlcd.h>
#include <stdlib.h>   /* calloc, strtol, getenv */
#include <string.h>   /* String function definitions */
#include <unistd.h>
#include <qegpioint.h>
#include <qeanalog.h>
#include "Metro.h"
#include "CQEI2C.h"
#include "ControlledMotor.h"
#include "qetime.h"
#include "ROSIF.h"
#include "JRoverMotors.h"
#include "JRoverSensors.h"
#include "Runtimer.h"

using namespace std;

#define PRINT_RATE 5

CQEI2C i2c = CQEI2C();		// instantiate the I2C driver
CQEGpioInt &gpio = CQEGpioInt::GetRef();
CQEAnalog &analog = CQEAnalog::GetRef();
JRoverMotors jRoverMotors = JRoverMotors();
JRoverSensors jRoverSensors = JRoverSensors();
CKeypad &keypad = CKeypad::GetRef();
CTextLcd &lcd = CTextLcd::GetRef();

// ROS definitions & instantiations
bool rosFlag;	// true to enable connection to ROS using rosserial
ROSIF rosif = ROSIF();


//int printRate = PRINT_RATE;

//#define ROS_RUN_TIME 3
//CQETime::tick_t rosCmdTime;				// time of last command from ROS - we won't run longer than ROS_RUN_TIME sec after it
Runtimer loopTimer = Runtimer((char*)"loopTimer");
Runtimer loop250Timer = Runtimer((char*)"loop250Timer");

void jRoverTest1()
{
	// ACHTUNG THIS IS JROVERTEST1!!!!!!!!!!! - NOT THE LATEST
#if 0
	float linear;
	float angular;
	float rcLinear, rcAngular;
	float rosLinear;
	float rosAngular;
	int estopWidth;


	// initialize ROS
	rosFlag = rosif.initRos();
	printf("test 1, ROS %s active\n", rosFlag?"IS":"IS NOT");

	lcd.Clear();
	lcd.printf("Press X to exit");

	jRoverMotors.initMotors();					// always start with this
	jRoverSensors.initSensors();	// and this

	// check motors every 50ms, and R/C every 250ms
	Metro metro = Metro(50);
	Metro metro250 = Metro(250);

	jRoverMotors.driveRover(0.0, 0.0);		// start off stopped
	while (!keypad.KeyCancel()){
		// ACHTUNG THIS IS JROVERTEST1!!!!!!!!!!! - NOT THE LATEST

		// every 250ms check for estop, manual override, ROS speed request, etc
		if (metro250.check()) {
			loop250Timer.start();

			// convert R/C values to desired speed range -20 - +20 ips & angle range +/-90
			rcLinear = 0.0 - (float)(jRoverSensors.getRcSpeed()-1550)/25.0;
			rcAngular = 0.0 - (float)(jRoverSensors.getRcDir()-1550)/1000.0;
			estopWidth = jRoverSensors.getRcEstop();

			// read ros speed request
			rosif.getRosLinearAngular(rosLinear, rosAngular);

			// if estop (R/C off), pulsewidth is 0, don't run
			// else if R/C indicates we're trying to drive it, let R/C have control
			// else let ROS have control
			// where is subsumption when you need it?

			// ACHTUNG THIS IS JROVERTEST1!!!!!!!!!!! - NOT THE LATEST

			if (estopWidth < 1700) {
				linear = angular = 0.0;
				printf("Estop prevents movement, estop: %d\n", estopWidth);
			} else if ((rcLinear < -4.0) || (rcLinear > 4.0)){
				linear = rcLinear;
				angular = rcAngular;
				printf("R/C control: linear: %0.1f, angular %0.2f (%d,%d)\n", linear, angular,
						jRoverSensors.getRcSpeed(), jRoverSensors.getRcDir());
			} else if ((rosLinear < -1.0) || (rosLinear > 1.0)) {
				linear = rosLinear;
				angular = rosAngular;
				printf("ROS control: linear: %0.1f, angular %0.2f, estop: %d\n", linear, angular, estopWidth);
			} else {
				linear = angular = 0.0;
				printf("No inputs request movement\n");
			}
			jRoverMotors.driveRover(linear, angular);
			loop250Timer.stop();
			if (!((loop250Timer.getCount())%100)) {
				loop250Timer.print();
			}
		}
		// ACHTUNG THIS IS JROVERTEST1!!!!!!!!!!! - NOT THE LATEST

		// every 50ms, check sensors, & update motors
		if (metro.check()) {		// if 50ms have passed & it's time to do the control loop
			loopTimer.start();

			jRoverSensors.pollSensors();			// poll the sensors
			jRoverMotors.updateAllMotors();			// run the PID loop


			loopTimer.stop();
			if (!((loopTimer.getCount())%100)) {
				loopTimer.print();
			}
		}
		// publish motor telemetry every 50ms if ROS is enabled
		if (rosif.rosEnabled) {
			rosif.poll();
		}
	}
#endif
}

void jRoverTest2()
{
	float linear, angular;

	printf("test 2, ROS %s active\n", rosFlag?"IS":"IS NOT");

	rosFlag = rosif.initRos();			// initialize ROS


	// check motors every 50ms, and generate a new speed/angle every 5 sec
	Metro metro = Metro(50);
	Metro metro5000 = Metro(5000);

	jRoverMotors.initMotors();					// always initialize the motors to start
	jRoverMotors.driveRover(0.0, 0.0);

	while(1) {
		if (metro5000.check()) {
			linear = (((float)(rand()%100) * 0.3) - 15.0);	// compute a random velocity -15 -> +15
			angular = (((float)(rand()%100) * 0.02) - 1.0);	// compute a random rotational velocity +/-2.0
			printf("linear: %0.1f, angular %0.1f\n", linear, angular);
			jRoverMotors.driveRover(linear, angular);
		}
		if (metro.check()) {		// if 50ms have passed & it's time to do the control loop
			jRoverMotors.updateAllMotors();			// run the PID loop

			// publish motor telemetry if ROS is enabled
//			if (rosFlag)
//				rosif.publishDriveMotorTelemetry();
//				rosif.publishSteerMotorTelemetry();
		}
	}
}

void jRoverTest3()
{
	float linear;
	float angular;
	float rcLinear, rcAngular;
	float rosLinear;
	float rosAngular;
	float lastRosLinear = 0.0;
	float lastRosAngular = 0.0;
	int estopWidth;
	int state = 0;

	// initialize ROS
	rosFlag = rosif.initRos();
	printf("test 1, ROS %s active\n", rosFlag?"IS":"IS NOT");

	lcd.Clear();
	lcd.printf("Press X to exit");

	jRoverMotors.initMotors();					// always start with this
	jRoverSensors.initSensors();	// and this

	// check motors every 50ms, and R/C every 250ms
	Metro metro = Metro(50);
	Metro metro250 = Metro(250);

	jRoverMotors.driveRover(0.0, 0.0);		// start off stopped
	while (!keypad.KeyCancel()){

		// every 250ms check for estop, manual override, ROS speed request, etc
		if (metro250.check()) {
			loop250Timer.start();

			// convert R/C values to desired speed range -20 - +20 ips & angle range +/-90
			rcLinear = 0.0 - (float)(jRoverSensors.getRcSpeed()-1550)/25.0;
			rcAngular = 0.0 - (float)(jRoverSensors.getRcDir()-1550)/1000.0;
			estopWidth = jRoverSensors.getRcEstop();

			// read ros speed request
			rosif.getRosLinearAngular(rosLinear, rosAngular);

			// if estop (R/C off), pulsewidth is 0, or estopPulsewidth < 1500, use R/C
			// else let ROS have control
			// where is subsumption when you need it?
#if 0
			if (0 && ((rcLinear < -4.0) || (rcLinear > 4.0)) && (estopWidth < 1500) && (estopWidth > 900)) {
				linear = rcLinear;
				angular = rcAngular;
				printf("R/C control: linear: %0.1f, angular %0.1f (%d,%d), estopWidth: %d\n",
						linear, angular,
						jRoverSensors.getRcSpeed(), jRoverSensors.getRcDir(), estopWidth);
				state = 2;
			} else if (((rosLinear < -1.0) || (rosLinear > 1.0)) && estopWidth > 1500) {
				linear = rosLinear;
				angular = rosAngular;
				//if ((state != 3) || (rosLinear != lastRosLinear) || (rosAngular != lastRosAngular)) {
					printf("ROS control: linear: %0.1f, angular %0.1f, estopWidth: %d\n",
						linear, angular, estopWidth);
				//}
				lastRosLinear = rosLinear;
				lastRosAngular = rosAngular;
				state = 3;
			} else {
				linear = angular = 0.0;
				if (state != 4)
					printf("No inputs request movement, estop: %d\n", estopWidth);
				state = 4;
			}
#endif
			if (((rosLinear < -0.1) || (rosLinear > 0.1))) {
				linear = rosLinear;
				angular = rosAngular;
				//if ((state != 3) || (rosLinear != lastRosLinear) || (rosAngular != lastRosAngular)) {
				printf("ROS control: linear: %0.1f, angular %0.1f, estopWidth: %d\n",
						linear, angular, estopWidth);
				//}
				lastRosLinear = rosLinear;
				lastRosAngular = rosAngular;
				state = 3;
			} else {
				linear = angular = 0.0;
				if (state != 4)
					printf("No inputs request movement, estop: %d\n", estopWidth);
				state = 4;
			}

			jRoverMotors.driveRover(linear, angular);
			loop250Timer.stop();
			if (!((loop250Timer.getCount())%1000)) {
				loop250Timer.print();
			}
		}

		// every 50ms, check sensors, & update motors
		if (metro.check()) {		// if 50ms have passed & it's time to do the control loop
			loopTimer.start();

			jRoverSensors.pollSensors();			// poll the sensors
			jRoverMotors.updateAllMotors();			// run the PID loop

			// publish motor telemetry every 50ms if ROS is enabled
			if (rosif.rosEnabled) {
				// drive & steer motor telemetry takes avg 42ms to collect & publish
				//jRoverMotors.getDriveMotorsTelemetry();
				//jRoverMotors.getSteerMotorsTelemetry();
				//rosif.publishDriveMotorTelemetry();
				//rosif.publishSteerMotorTelemetry();

				// RobotTelemetry takes avg 9 ms to publish
				rosif.publishRobotTelemetry();
			}

			loopTimer.stop();
			if (!((loopTimer.getCount())%1000)) {
				loopTimer.print();
			}
		}
		// publish motor telemetry every 50ms if ROS is enabled
		if (rosif.rosEnabled) {
			rosif.poll();
		}
	}
}

void usage()
{
    printf("Usage: jRover [OPTIONS]\n"
    "\n"
    "Options:\n"
    "  -h, --help                   Print this help message\n"
    "  -t<testnum>					Run the specified test\n"
    "Tests:\n"
    "1: Drive jRover from R/C or ROS\n"
    "2: Random drive speed & directions\n"
    "3: Drive jRover ROS with telemetry sent to ROS\n"
    "\n"
    );
}


int main(int argc, char *argv[])
{
	int i;
	int testNum = DEFAULT_TEST;			// which test to execute

	// if no arguments (e.g. run from TerkIDE) use default arguments from list above
    if (argc == 1) {
    	printf("No args provided; using default: ");
    	for(i=0; i<default_argc; i++) {
    		printf("%s ", default_argv[i]);
    	}
    	printf("\n");
    	argc = default_argc;
    	argv = default_argv;
    }

    // parse options & arguments
    int option_index = 0, opt;
    static struct option loptions[] = {
        {"help",       no_argument,       0, 'h'},
        {"ros",			no_argument,	0, 'r'},
        {0,0,0,0}
    };

    while(1) {
        opt = getopt_long (argc, argv, "hrt:",
                           loptions, &option_index);
        if (opt==-1) break;

        switch (opt) {
        case 'h': usage(); break;
        case 'r': rosFlag = true; break;
        case 't': testNum = strtol(optarg, NULL, 10); break;
        default: usage(); exit(0);
        }
    }

    switch (testNum) {
    case 1: jRoverTest1(); break;
    case 2: jRoverTest2(); break;
    case 3: jRoverTest3(); break;
//    case 4: jRoverTest4(); break;
//    case 5: jRoverTest5(); break;
//    case 6: jRoverTest6(); break;
    default: printf("Invalid test number\n"); exit(0);
    }
}
